frame_base.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
// Fwd refs
namespace wolf
{
class TrajectoryBase;
class CaptureBase;
class StateBlock;
}  // namespace wolf
 
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/time_stamp.h"
#include "core/common/node_state_blocks.h"
 
namespace wolf
{
// class FrameBase
class FrameBase : public NodeStateBlocks
{
    friend CaptureBase;
    friend FactorBase;
 
  private:
    TrajectoryBaseWPtr trajectory_ptr_;
    CaptureBasePtrList capture_list_;
 
    static unsigned int frame_id_count_;
 
  protected:
    unsigned int frame_id_;
    TimeStamp    time_stamp_;  
 
  public:
    FrameBase(const TimeStamp&       _ts,
              const TypeComposite&   _frame_types,
              const VectorComposite& _frame_vectors,
              const PriorComposite&  _frame_priors = {});
 
    ~FrameBase() override = default;
 
    // Add and remove from WOLF tree ---------------------------------
    template <typename classType, typename... T>
    static std::shared_ptr<classType> emplace(TrajectoryBasePtr _ptr, T&&... all);
 
    void link(TrajectoryBasePtr);
    void link(ProblemPtr _prb);
 
    void remove(bool viral_remove_parent_without_children = false) override;
    bool hasChildren() const override;
 
    // Frame properties -----------------------------------------------
  public:
    unsigned int id() const override;
 
    // Frame values ------------------------------------------------
  public:
    void      setTimeStamp(const TimeStamp& _ts);
    TimeStamp getTimeStamp() const;
    void      getTimeStamp(TimeStamp& _ts) const;
 
    // State blocks ----------------------------------------------------
  public:
    StateBlockConstPtr getV() const;
    StateBlockPtr      getV();
 
  protected:
    void setProblem(ProblemPtr _problem) override final;
 
    // Wolf tree access ---------------------------------------------------
  public:
    TrajectoryBaseConstPtr getTrajectory() const;
    TrajectoryBasePtr      getTrajectory();
 
    FrameBaseConstPtr getPreviousFrame(const unsigned int& i = 1) const;
    FrameBaseConstPtr getNextFrame(const unsigned int& i = 1) const;
    FrameBasePtr      getPreviousFrame(const unsigned int& i = 1);
    FrameBasePtr      getNextFrame(const unsigned int& i = 1);
 
    CaptureBaseConstPtrList getCaptureList() const;
    CaptureBasePtrList      getCaptureList();
    bool                    hasCapture(const CaptureBaseConstPtr& _capture) const;
 
    FactorBaseConstPtrList getFactorList() const;
    FactorBasePtrList      getFactorList();
    void                   getFactorList(FactorBaseConstPtrList& _fac_list) const;
    void                   getFactorList(FactorBasePtrList& _fac_list);
 
    template <class C>
    std::shared_ptr<const C> getCaptureOfType() const;
    template <class C>
    std::shared_ptr<C> getCaptureOfType();
 
    CaptureBaseConstPtr getCaptureOfType(const std::string& type) const;
    CaptureBasePtr      getCaptureOfType(const std::string& type);
 
    template <class C>
    std::list<std::shared_ptr<const C>> getCapturesOfType() const;
    template <class C>
    std::list<std::shared_ptr<C>> getCapturesOfType();
 
    CaptureBaseConstPtrList getCapturesOfType(const std::string& type) const;
    CaptureBasePtrList      getCapturesOfType(const std::string& type);
 
    CaptureBaseConstPtr getCaptureOf(SensorBaseConstPtr _sensor_ptr) const;
    CaptureBasePtr      getCaptureOf(SensorBaseConstPtr _sensor_ptr);
 
    CaptureBaseConstPtr getCaptureOf(SensorBaseConstPtr _sensor_ptr, const std::string& type) const;
    CaptureBasePtr      getCaptureOf(SensorBasePtr _sensor_ptr, const std::string& type);
 
    CaptureBaseConstPtrList getCapturesOf(SensorBaseConstPtr _sensor_ptr) const;
    CaptureBasePtrList      getCapturesOf(SensorBasePtr _sensor_ptr);
 
    // Debug and info -------------------------------------------------------
    virtual void printHeader(int           depth,        //
                             bool          factored_by,  //
                             bool          metric,       //
                             bool          state_blocks,
                             std::ostream& stream,
                             std::string   _tabs = "") const;
 
    void print(int           depth,        //
               bool          factored_by,  //
               bool          metric,       //
               bool          state_blocks,
               std::ostream& stream = std::cout,
               std::string   _tabs  = "") const;
 
    virtual CheckLog localCheck(bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
    bool             check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
 
  private:
    CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr);
    void           removeCapture(CaptureBasePtr _capt_ptr);
    void           setTrajectory(TrajectoryBasePtr _trj_ptr);
 
  public:
    FrameBasePtr shared_from_this_frame()
    {
        return std::static_pointer_cast<FrameBase>(shared_from_this());
    };
    FrameBaseConstPtr shared_from_this_frame() const
    {
        return std::static_pointer_cast<const FrameBase>(shared_from_this());
    };
};
 
}  // namespace wolf
 
// IMPLEMENTATION //
 
#include "core/trajectory/trajectory_base.h"
#include "core/capture/capture_base.h"
#include "core/factor/factor_base.h"
#include "core/state_block/state_block.h"
 
namespace wolf
{
template <typename classType, typename... T>
std::shared_ptr<classType> FrameBase::emplace(TrajectoryBasePtr _ptr, T&&... all)
{
    std::shared_ptr<classType> frm = std::make_shared<classType>(std::forward<T>(all)...);
    frm->emplacePriors();
    frm->link(_ptr);
    return frm;
}
 
inline unsigned int FrameBase::id() const
{
    return frame_id_;
}
 
inline void FrameBase::getTimeStamp(TimeStamp& _ts) const
{
    _ts = time_stamp_;
}
 
inline TimeStamp FrameBase::getTimeStamp() const
{
    return time_stamp_;
}
 
inline StateBlockConstPtr FrameBase::getV() const
{
    return getStateBlock('V');
}
 
inline StateBlockPtr FrameBase::getV()
{
    return getStateBlock('V');
}
 
inline TrajectoryBaseConstPtr FrameBase::getTrajectory() const
{
    return trajectory_ptr_.lock();
}
 
inline TrajectoryBasePtr FrameBase::getTrajectory()
{
    return trajectory_ptr_.lock();
}
 
inline CaptureBaseConstPtrList FrameBase::getCaptureList() const
{
    CaptureBaseConstPtrList list_const;
    for (auto&& obj_ptr : capture_list_) list_const.push_back(obj_ptr);
    return list_const;
}
 
inline CaptureBasePtrList FrameBase::getCaptureList()
{
    return capture_list_;
}
 
inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr)
{
    trajectory_ptr_ = _trj_ptr;
}
 
template <class C>
inline std::shared_ptr<const C> FrameBase::getCaptureOfType() const
{
    for (auto capture_ptr : getCaptureList())
    {
        auto cap_derived = std::dynamic_pointer_cast<const C>(capture_ptr);
        if (cap_derived) return cap_derived;
    }
    return nullptr;
}
 
template <class C>
inline std::shared_ptr<C> FrameBase::getCaptureOfType()
{
    for (auto capture_ptr : getCaptureList())
    {
        auto cap_derived = std::dynamic_pointer_cast<C>(capture_ptr);
        if (cap_derived) return cap_derived;
    }
    return nullptr;
}
 
template <class C>
inline std::list<std::shared_ptr<const C>> FrameBase::getCapturesOfType() const
{
    std::list<std::shared_ptr<const C>> captures;
    for (auto capture_ptr : getCaptureList())
    {
        auto cap_derived = std::dynamic_pointer_cast<const C>(capture_ptr);
        if (cap_derived) captures.push_back(cap_derived);
    }
    return captures;
}
 
template <class C>
inline std::list<std::shared_ptr<C>> FrameBase::getCapturesOfType()
{
    std::list<std::shared_ptr<C>> captures;
    for (auto capture_ptr : getCaptureList())
    {
        auto cap_derived = std::dynamic_pointer_cast<C>(capture_ptr);
        if (cap_derived) captures.push_back(cap_derived);
    }
    return captures;
}
 
}  // namespace wolf